#!/usr/bin/env python
import rospy
import tkinter as tk
from mavros_msgs.msg import RCIn

class RcController:
    def __init__(self, master):
        self.master = master
        self.master.title("16-Channel RC Controller")

        # 初始化 MAVROS 发布者
        self.pub = rospy.Publisher('/mavros/rc/in', RCIn, queue_size=10)
        self.rc_msg = RCIn()

        # 创建一个字典来存储每个滑块的值
        self.sliders = []
        self.labels = []  # 存储每个滑块对应的值标签

        # 创建 16 个滑块并布局
        for i in range(16):
            label = tk.Label(self.master, text=f"Channel {i + 1}")
            label.grid(row=i, column=0)

            # 创建滑块，设置范围为1000到2000，默认值1500
            slider = tk.Scale(self.master, from_=1000, to=2000, orient="horizontal", resolution=10)
            slider.set(1500)
            slider.grid(row=i, column=1)

            # 创建一个标签来显示滑块的当前值
            value_label = tk.Label(self.master, text=str(slider.get()))
            value_label.grid(row=i, column=2)
            self.labels.append(value_label)

            # 绑定滑块的值变化事件
            slider.bind("<Motion>", lambda event, idx=i: self.update_value(idx))

            # 将滑块加入列表
            self.sliders.append(slider)

        # 启动一个定时器，周期性地发布 RC 信号
        self.update_rc_signal()

    def update_value(self, idx):
        """实时更新每个滑块的值到相应的标签"""
        current_value = self.sliders[idx].get()
        self.labels[idx].config(text=str(current_value))

    def update_rc_signal(self):
        """定期获取所有滑块的值并实时发布 RC 信号"""
        # 获取滑块的值并将它们设置为 RCIn 消息的通道值
        self.rc_msg.channels = [slider.get() for slider in self.sliders]

        # 发布 RC 消息
        self.pub.publish(self.rc_msg)
        rospy.loginfo(f"Publishing RC signal: {self.rc_msg.channels}")

        # 每 0.1 秒发布一次 RC 信号，确保实时性
        self.master.after(100, self.update_rc_signal)

def main():
    # 初始化 ROS 节点
    rospy.init_node('rc_gui_publisher', anonymous=True)
    
    # 创建 Tkinter 窗口
    root = tk.Tk()

    # 创建遥控器控制器实例
    rc_controller = RcController(root)

    # 启动 Tkinter 主循环
    root.mainloop()

if __name__ == '__main__':
    try:
        main()
    except rospy.ROSInterruptException:
        pass
